#include "lmpanalysis.h"

lmp_pdu_msg_t lmp_pdu_msg_table[LMP_PDU_MSG_NUM] = {
// length    pck_type   LMP PUD
    {2  << 1 | 0 ,"LMP_ACCEPTED"},
    {4  << 1 | 1 ,"LMP_ACCEPTED_EXT"},
    {17 << 1 | 1 ,"LMP_AU_RAND"},
    {1  << 1 | 0 ,"LMP_AUTO_RATE"},
    {12 << 1 | 1 ,"LMP_CHANNEL_CLASSIFICATION"},
    {7  << 1 | 1 ,"LMP_CHANNEL_CLASSIFICATION_REQ"},
    {15 << 1 | 1 ,"LMP_CLK_ADJ"},
    {3  << 1 | 1 ,"LMP_CLK_ADJ_ACK"},
    {6  << 1 | 1 ,"LMP_CLK_ADJ_REQ"},
    {1  << 1 | 0 ,"LMP_CLKOFFSET_REQ"},
    {3  << 1 | 0 ,"LMP_CLKOFFSET_RES"},
    {17 << 1 | 1 ,"LMP_COMB_KEY"},
    {2  << 1 | 0 ,"LMP_DECR_POWER_REQ"},
    {2  << 1 | 0 ,"LMP_DETACH"},
    {17 << 1 | 1 ,"LMP_DHKEY_CHECK"},
    {4  << 1 | 1 ,"LMP_ENCAPSULATED_HEADER"},
    {17 << 1 | 1 ,"LMP_ENCAPSULATED_PAYLOAD"},
    {1  << 1 | 1 ,"LMP_ENCRYPTION_KEY_SIZE_MASK_REQ"},
    {3  << 1 | 1 ,"LMP_ENCRYPTION_KEY_SIZE_MASK_RES"},
    {2  << 1 | 0 ,"LMP_ENCRYPTION_KEY_SIZE_REQ"},
    {2  << 1 | 0 ,"LMP_ENCRYPTION_MODE_REQ"},
    {16 << 1 | 1 ,"LMP_eSCO_LINK_REQ"},
    {9  << 1 | 0 ,"LMP_FEATURES_REQ"},
    {12 << 1 | 1 ,"LMP_FEATURES_REQ_EXT"},
    {9  << 1 | 0 ,"LMP_FEATURES_RES"},
    {12 << 1 | 1 ,"LMP_FEATURES_RES_EXT"},
    {7  << 1 | 0 ,"LMP_HOLD"},
    {7  << 1 | 0 ,"LMP_HOLD_REQ"},
    {1  << 1 | 0 ,"LMP_HOST_CONNECTION_REQ"},
    {17 << 1 | 1 ,"LMP_IN_RAND"},
    {2  << 1 | 0 ,"LMP_INCR_POWER_REQ"},
    {5  << 1 | 1 ,"LMP_IO_CAPABILITY_REQ"},
    {5  << 1 | 1 ,"LMP_IO_CAPABILITY_RES"},
    {3  << 1 | 1 ,"LMP_KEYPRESS_NOTIFICATION"},
    {1  << 1 | 0 ,"LMP_MAX_POWER"},
    {2  << 1 | 0 ,"LMP_MAX_SLOT"},
    {2  << 1 | 0 ,"LMP_MAX_SLOT_REQ"},
    {1  << 1 | 0 ,"LMP_MIN_POWER"},
    {2  << 1 | 0 ,"LMP_NAME_REQ"},
    {17 << 1 | 1 ,"LMP_NAME_RES"},
    {3  << 1 | 0 ,"LMP_NOT_ACCEPTED"},
    {5  << 1 | 1 ,"LMP_NOT_ACCEPTED_EXT"},
    {2  << 1 | 1 ,"LMP_NUMERIC_COMPARISON_FAILED"},
    {2  << 1 | 1 ,"LMP_OOB_FAILED"},
    {3  << 1 | 1 ,"LMP_PACKET_TYPE_TABLE_REQ"},
    {3  << 1 | 0 ,"LMP_PAGE_MODE_REQ"},
    {3  << 1 | 0 ,"LMP_PAGE_SCAN_MODE_REQ"},
    {2  << 1 | 1 ,"LMP_PASSKEY_FAILED"},
    {17 << 1 | 1 ,"LMP_PAUSE_ENCRYPTION_AES_REQ"},
    {2  << 1 | 1 ,"LMP_PAUSE_ENCRYPTION_REQ"},
    {2  << 1 | 1 ,"LMP_PING_REQ"},
    {2  << 1 | 1 ,"LMP_PING_RES"},
    {3  << 1 | 0 ,"LMP_POWER_CONTROL_REQ"},
    {3  << 1 | 0 ,"LMP_POWER_CONTROL_RES"},
    {2  << 1 | 0 ,"LMP_PREFERRED_RATE"},
    {4  << 1 | 0 ,"LMP_QUALITY_OF_SERVICE"},
    {4  << 1 | 0 ,"LMP_QUALITY_OF_SERVICE_REQ"},
    {4  << 1 | 1 ,"LMP_REMOVE_eSCO_LINK_REQ"},
    {3  << 1 | 0 ,"LMP_REMOVE_SCO_LINK_REQ"},
    {2  << 1 | 1 ,"LMP_RESUME_ENCRYPTION_REQ"},
    {17 << 1 | 1 ,"LMP_SAM_DEFINE_MAP"},
    {17 << 1 | 1 ,"LMP_SAM_SET_TYPE0"},
    {9  << 1 | 1 ,"LMP_SAM_SWITCH"},
    {7  << 1 | 0 ,"LMP_SCO_LINK_REQ"},
    {16 << 1 | 1 ,"LMP_SET_AFH"},
    {1  << 1 | 1 ,"LMP_SETUP_COMPLETE"},
    {17 << 1 | 1 ,"LMP_SIMPLE_PAIRING_CONFIRM"},
    {17 << 1 | 1 ,"LMP_SIMPLE_PAIRING_NUMBER"},
    {9  << 1 | 0 ,"LMP_SLOT_OFFSET"},
    {10 << 1 | 1 ,"LMP_SNIFF_REQ"},
    {9  << 1 | 1 ,"LMP_SNIFF_SUBRATING_REQ"},
    {9  << 1 | 1 ,"LMP_SNIFF_SUBRATING_RES"},
    {5  << 1 | 0 ,"LMP_SRES"},
    {17 << 1 | 1 ,"LMP_START_ENCRYPTION_REQ"},
    {1  << 1 | 0 ,"LMP_STOP_ENCRYPTION_REQ"},
    {3  << 1 | 0 ,"LMP_SUPERVISION_TIMEOUT"},
    {5  << 1 | 1 ,"LMP_SWITCH_REQ"},
    {17 << 1 | 1 ,"LMP_TEMP_KEY"},
    {17 << 1 | 1 ,"LMP_TEMP_RAND"},
    {1  << 1 | 0 ,"LMP_TEST_ACTIVATE"},
    {10 << 1 | 1 ,"LMP_TEST_CONTROL"},
    {1  << 1 | 0 ,"LMP_TIMING_ACCURACY_REQ"},
    {3  << 1 | 0 ,"LMP_TIMING_ACCURACY_RES"},
    {17 << 1 | 1 ,"LMP_UNIT_KEY"},
    {1  << 1 | 0 ,"LMP_UNSNIFF_REQ"},
    {1  << 1 | 0 ,"LMP_USE_SEMI_PERMANENT_KEY"},
    {6  << 1 | 0 ,"LMP_VERSION_REQ"},
    {6  << 1 | 0 ,"LMP_VERSION_RES"},
};

lmp_pdu_t lmp_pdu_table[LMP_PDU_MSG_NUM]={
//        opcode           lmp_msg
    {0 << 8   | 3  ,lmp_pdu_msg_table[0]},
    {127 << 8 | 1  ,lmp_pdu_msg_table[1]},
    {0 << 8   | 11 ,lmp_pdu_msg_table[2]},
    {0 << 8   | 35 ,lmp_pdu_msg_table[3]},
    {127 << 8 | 17 ,lmp_pdu_msg_table[4]},
    {127 << 8 | 16 ,lmp_pdu_msg_table[5]},
    {127 << 8 | 5  ,lmp_pdu_msg_table[6]},
    {127 << 8 | 6  ,lmp_pdu_msg_table[7]},
    {127 << 8 | 7  ,lmp_pdu_msg_table[8]},
    {0 << 8   | 5  ,lmp_pdu_msg_table[9]},
    {0 << 8   | 6  ,lmp_pdu_msg_table[10]},
    {0 << 8   | 9  ,lmp_pdu_msg_table[11]},
    {0 << 8   | 32 ,lmp_pdu_msg_table[12]},
    {0 << 8   | 7  ,lmp_pdu_msg_table[13]},
    {0 << 8   | 65 ,lmp_pdu_msg_table[14]},
    {0 << 8   | 61 ,lmp_pdu_msg_table[15]},
    {0 << 8   | 62 ,lmp_pdu_msg_table[16]},
    {0 << 8   | 58 ,lmp_pdu_msg_table[17]},
    {0 << 8   | 59 ,lmp_pdu_msg_table[18]},
    {0 << 8   | 16 ,lmp_pdu_msg_table[19]},
    {0 << 8   | 15 ,lmp_pdu_msg_table[20]},
    {127 << 8 | 12 ,lmp_pdu_msg_table[21]},
    {0 << 8   | 39 ,lmp_pdu_msg_table[22]},
    {127 << 8 | 3  ,lmp_pdu_msg_table[23]},
    {0 << 8   | 40 ,lmp_pdu_msg_table[24]},
    {127 << 8 | 04 ,lmp_pdu_msg_table[25]},
    {0 << 8   | 20 ,lmp_pdu_msg_table[26]},
    {0 << 8   | 21 ,lmp_pdu_msg_table[27]},
    {0 << 8   | 51 ,lmp_pdu_msg_table[28]},
    {0 << 8   | 8  ,lmp_pdu_msg_table[29]},
    {0 << 8   | 31 ,lmp_pdu_msg_table[30]},
    {127 << 8 | 25 ,lmp_pdu_msg_table[31]},
    {127 << 8 | 26 ,lmp_pdu_msg_table[32]},
    {127 << 8 | 30 ,lmp_pdu_msg_table[33]},
    {0 << 8   | 33 ,lmp_pdu_msg_table[34]},
    {0 << 8   | 45 ,lmp_pdu_msg_table[35]},
    {0 << 8   | 46 ,lmp_pdu_msg_table[36]},
    {0 << 8   | 34 ,lmp_pdu_msg_table[37]},
    {0 << 8   | 1  ,lmp_pdu_msg_table[38]},
    {0 << 8   | 2  ,lmp_pdu_msg_table[39]},
    {0 << 8   | 4  ,lmp_pdu_msg_table[40]},
    {127 << 8 | 2  ,lmp_pdu_msg_table[41]},
    {127 << 8 | 27 ,lmp_pdu_msg_table[42]},
    {127 << 8 | 29 ,lmp_pdu_msg_table[43]},
    {127 << 8 | 11 ,lmp_pdu_msg_table[44]},
    {0 <<8    | 53 ,lmp_pdu_msg_table[45]},
    {0 <<8    | 54 ,lmp_pdu_msg_table[46]},
    {127 << 8 | 28 ,lmp_pdu_msg_table[47]},
    {0 << 8   | 66 ,lmp_pdu_msg_table[48]},
    {127 << 8 | 23 ,lmp_pdu_msg_table[49]},
    {127 << 8 | 33 ,lmp_pdu_msg_table[50]},
    {127 << 8 | 34 ,lmp_pdu_msg_table[51]},
    {127 << 8 | 31 ,lmp_pdu_msg_table[52]},
    {127 << 8 | 32 ,lmp_pdu_msg_table[53]},
    {0 << 8   | 36 ,lmp_pdu_msg_table[54]},
    {0 << 8   | 41 ,lmp_pdu_msg_table[55]},
    {0 << 8   | 42 ,lmp_pdu_msg_table[56]},
    {127 << 8 | 13 ,lmp_pdu_msg_table[57]},
    {0 << 8   | 44 ,lmp_pdu_msg_table[58]},
    {127 << 8 | 24 ,lmp_pdu_msg_table[59]},
    {127 << 8 | 36 ,lmp_pdu_msg_table[60]},
    {127 << 8 | 35 ,lmp_pdu_msg_table[61]},
    {127 << 8 | 37 ,lmp_pdu_msg_table[62]},
    {0 << 8   | 43 ,lmp_pdu_msg_table[63]},
    {0 << 8   | 60 ,lmp_pdu_msg_table[64]},
    {0 << 8   | 49 ,lmp_pdu_msg_table[65]},
    {0 << 8   | 63 ,lmp_pdu_msg_table[66]},
    {0 << 8   | 64 ,lmp_pdu_msg_table[67]},
    {0 << 8   | 52 ,lmp_pdu_msg_table[68]},
    {0 << 8   | 23 ,lmp_pdu_msg_table[69]},
    {127 << 8 | 21 ,lmp_pdu_msg_table[70]},
    {127 << 8 | 22 ,lmp_pdu_msg_table[71]},
    {0 << 8   | 12 ,lmp_pdu_msg_table[72]},
    {0 << 8   | 17 ,lmp_pdu_msg_table[73]},
    {0 << 8   | 18 ,lmp_pdu_msg_table[74]},
    {0 << 8   | 55 ,lmp_pdu_msg_table[75]},
    {0 << 8   | 19 ,lmp_pdu_msg_table[76]},
    {0 << 8   | 14 ,lmp_pdu_msg_table[77]},
    {0 << 8   | 13 ,lmp_pdu_msg_table[78]},
    {0 << 8   | 56 ,lmp_pdu_msg_table[79]},
    {0 << 8   | 57 ,lmp_pdu_msg_table[80]},
    {0 << 8   | 47 ,lmp_pdu_msg_table[81]},
    {0 << 8   | 48 ,lmp_pdu_msg_table[82]},
    {0 << 8   | 10 ,lmp_pdu_msg_table[83]},
    {0 << 8   | 24 ,lmp_pdu_msg_table[84]},
    {0 << 8   | 50 ,lmp_pdu_msg_table[85]},
    {0 << 8   | 37 ,lmp_pdu_msg_table[86]},
    {0 << 8   | 38 ,lmp_pdu_msg_table[87]},
};

lmpanalysis::lmpanalysis()
{
    for(int i = 0; i < LMP_PDU_MSG_NUM; i++){
        lmp_map.insert(pair<unsigned short,lmp_pdu_msg_t>(lmp_pdu_table[i].opcode,lmp_pdu_table[i].lmp_pdu_msg));
    }
//    map<unsigned short,lmp_pdu_msg_t>::iterator pos = lmp_map.find(14);
//    if(pos != lmp_map.end()){
//        cout <<  (*pos).first << endl;
//        cout <<  (*pos).second.pck_msg << endl;
//        cout <<  (*pos).second.pdu_msg << endl;
//    }
}

void lmpanalysis::getlmpmsg(const string &lmp_msg, string &pkg_type, string &pud, string &pos_dir){
    string lmp_msg_head = lmp_msg.substr(0,2);
    if(lmp_msg_head == "fe" || lmp_msg_head == "ff"){
        unsigned short *esc_opcode_t = reinterpret_cast<unsigned short*>(const_cast<char*>(lmp_msg_head.c_str()));
        esc_opcode = *esc_opcode_t >> 1;
        lmp_pkg_dir = (*esc_opcode_t) & 0x01;
        lmp_msg_head = lmp_msg.substr(3,2);
        unsigned short *ext_opcode_t = reinterpret_cast<unsigned short*>(const_cast<char*>(lmp_msg_head.c_str()));
        ext_opcode = *ext_opcode_t;
        opcode = esc_opcode << 8 | ext_opcode;
    } else {
        esc_opcode = 0;
        unsigned short *ext_opcode_t = reinterpret_cast<unsigned short*>(const_cast<char*>(lmp_msg_head.c_str()));
        ext_opcode = *ext_opcode_t >> 1;
        opcode = esc_opcode << 8 | ext_opcode;
        lmp_pkg_dir = (*ext_opcode_t) & 0x01;
    }
    map<unsigned short,lmp_pdu_msg_t>::iterator pos = lmp_map.find(opcode);
    if(pos != lmp_map.end()){
        pud = (*pos).second.pdu_msg;
        lmp_pkg_type = (*pos).second.pck_msg & 0x01 ;
    }
    pkg_type = lmp_pkg_type ? "DM1" : "DM1/DV";
    pos_dir = lmp_pkg_dir ? "slave -> master" : "master -> slave";
}

lmpanalysis::~lmpanalysis(){
    lmp_map.clear();
}
